#include "communication/communication_plugin.h"
#include <pluginlib/class_list_macros.h>
#include "smarteye_common/CommandLong.h"
#include "smarteye_common/CommandAck.h"
#include "smarteye_common/CommandAckCustom.h"


namespace commplugin {

class CommandPlugin : public CommPlugin {
public:
        CommandPlugin() :
                command_nh("~"),
                station_link(nullptr)
        { };

        void initialize(augmavconn::AugMavConnInterface::AugPtr station_link_, int target_system_id_, int target_component_id_)
        {
            station_link = station_link_;
            target_system_id = target_system_id_;
            target_component_id = target_component_id_;
            command_pub = command_nh.advertise<smarteye_common::CommandLong>("command_long", 10, true);
            commandAck_sub = command_nh.subscribe("/commander/command_ack",10, &CommandPlugin::receiveCommandAck, this);
            commandAckCustom_sub = command_nh.subscribe("/commander/command_ack_custom",10, &CommandPlugin::receiveCommandAckCustom, this);
        }

        const message_map get_rx_handlers() {
                return {
                               MESSAGE_HANDLER(MAVLINK_MSG_ID_COMMAND_LONG, &CommandPlugin::handle_command_long)
                };
        }

private:
        ros::NodeHandle command_nh;
        augmavconn::AugMavConnInterface::AugPtr station_link;
        int target_system_id;
        int target_component_id;
        ros::Publisher command_pub;
        ros::Subscriber commandAck_sub;
        ros::Subscriber commandAckCustom_sub;



        void handle_command_long(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid)
        {
            mavlink_command_long_t comLongMsg;
            mavlink_msg_command_long_decode(msg,&comLongMsg);

            if (comLongMsg.target_system != station_link->get_system_id()
                    || comLongMsg.target_component != station_link->get_component_id())
            {
                return;
            }

            smarteye_common::CommandLong comLong_rmsg;
            comLong_rmsg.command = comLongMsg.command;
            comLong_rmsg.confirmation = comLongMsg.confirmation;
            comLong_rmsg.param1 = comLongMsg.param1;
            comLong_rmsg.param2 = comLongMsg.param2;
            comLong_rmsg.param3 = comLongMsg.param3;
            comLong_rmsg.param4 = comLongMsg.param4;
            comLong_rmsg.param5 = comLongMsg.param5;
            comLong_rmsg.param6 = comLongMsg.param6;
            comLong_rmsg.param7 = comLongMsg.param7;
            command_pub.publish(comLong_rmsg);
        }

        void receiveCommandAck(const smarteye_common::CommandAck& msg)
        {
            smarteye_common::CommandAck commandAck;
            commandAck.command = msg.command;
            commandAck.result = msg.result;
            sendCommandAck(commandAck);
        }

        void receiveCommandAckCustom(const smarteye_common::CommandAckCustom& msg)
        {
            smarteye_common::CommandAckCustom commandAckCustom;
            commandAckCustom.command = msg.command;
            commandAckCustom.detectResult = msg.detectResult;
            commandAckCustom.host_version = msg.host_version;
            sendCommandAckCustom(commandAckCustom);
        }

        void sendCommandAck(smarteye_common::CommandAck& commandAck)
        {
            mavlink_message_t msg;
            mavlink_msg_command_ack_pack_chan(PACK_CHAN(station_link),
                                              &msg,
                                              commandAck.command,
                                              commandAck.result);
            station_link->send_message(&msg);
            ROS_DEBUG("send command_ack");

        }

        void sendCommandAckCustom(smarteye_common::CommandAckCustom& commandAckCustom)
        {
            mavlink_message_t msg;
            char host_vertion_char[sizeof(mavlink_command_ack_custom_t::host_version)];
            strncpy(host_vertion_char, commandAckCustom.host_version.c_str(), sizeof(host_vertion_char));

            mavlink_msg_command_ack_custom_pack_chan(PACK_CHAN(station_link),
                                              &msg,
                                              commandAckCustom.command,
                                              commandAckCustom.detectResult,
                                              host_vertion_char
                                                     );
            station_link->send_message(&msg);
            ROS_DEBUG("send command_ack_custom");

        }

};


};


PLUGINLIB_EXPORT_CLASS(commplugin::CommandPlugin, commplugin::CommPlugin)

